#include"ros/ros.h"
#include"turtlesim/Pose.h"
#include"tf2_ros/transform_broadcaster.h"
#include"geometry_msgs/TransformStamped.h"
#include"tf2/LinearMath/Quaternion.h"
#include"std_msgs/String.h"

std::string turtle_name;

void domsg(turtlesim::Pose::ConstPtr p)
{
    //需要构建成静态的发布对象
    static tf2_ros::TransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped msg;
    msg.child_frame_id=turtle_name;
    msg.header.frame_id="world";
    msg.header.stamp=ros::Time::now();
    msg.transform.translation.x=p->x;
    msg.transform.translation.y=p->y;
    msg.transform.translation.z=0;
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,p->theta);
    msg.transform.rotation.x=qtn.getX();
    msg.transform.rotation.y=qtn.getY();
    msg.transform.rotation.z=qtn.getZ();
    msg.transform.rotation.w=qtn.getW();
    broadcaster.sendTransform(msg);

}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"edg");
    ros::NodeHandle nh;
    if(argc!=2)
    {
        ROS_INFO("error:%d",argc);
        return 1;
    }
    else
    {
        turtle_name=argv[1];
         ROS_INFO("error:%s",argv[1]);
    }
    ros::Subscriber sub=nh.subscribe(turtle_name+"/pose",100,domsg);
    ros::spin();
    
    return 0;
}
